#include <iostream>
#include <string>
#include <vector>
#include <cstdio>
using namespace std;

#include "Propulsion.h"
CPropulsion::CPropulsion()
{
	this->_statePrev.x = 1;
	this->_statePrev.y = 0;
	this->_statePrev.z = 0;
}
CPropulsion::CPropulsion(FUNC_PTR_Propulsion fptrPropulsion)
{
	this->_fptrPropulsion = fptrPropulsion;
}

CPropulsion::~CPropulsion()
{
}
bool CPropulsion::OnNewMail(MOOSMSG_LIST &NewMail)
{
	MOOSMSG_LIST::iterator iterMSG;
	for(iterMSG = NewMail.begin(); iterMSG != NewMail.end(); iterMSG++){
		CMOOSMsg & rMsg = *iterMSG;
		if(MOOSStrCmp(rMsg.GetKey(),"Control"))
		{
	 		this->OnControl(rMsg);
		}
		if(MOOSStrCmp(rMsg.GetKey(),"State"))
		{
	 		this->OnState(rMsg);
		}
	}

	return true;
}

bool CPropulsion::OnConnectToServer()
{
		this->DoRegistrations();
        return true;
}
bool CPropulsion::Iterate()
{
        return true;
}
bool CPropulsion::OnStartUp()
{
		this->DoRegistrations();
        return true;
}

void CPropulsion::DoRegistrations(){
	this->m_Comms.Register("Control", 0);
	//this->m_Comms.Register("State", 0);
	return;
}

bool CPropulsion::OnControl(CMOOSMsg& msg){

	//msg.Trace(); //print msg in details
	cout << "RCVD: " << msg.GetString() << endl;
	int dblControlCount;
	string strControl = msg.GetString();
	this->_ctrlPrev.clear();

	MOOSValFromString (dblControlCount , strControl, "ControlCount", false);

	cout << dblControlCount << endl;
	char buff[10];
	for(int i=1; i <= (int)dblControlCount; i++){
		sprintf(buff, "control%d", i);
		double dblControl;
		MOOSValFromString (dblControl , strControl, buff, false);
		_ctrlPrev.push_back(dblControl);
	}

	vector<double> vdblPropulsions;
	this->_fptrPropulsion(this->_ctrlPrev, this->_statePrev, vdblPropulsions);

    string	strPropulsion;
	strPropulsion = MOOSFormat("tau1=%f,tau2=%f,tau3=%f,tau4=%f,tau5=%f,tau6=%f",
		vdblPropulsions[0],
		vdblPropulsions[1],
		vdblPropulsions[2],
		vdblPropulsions[3],
		vdblPropulsions[4],
		vdblPropulsions[5]);

	cout << "SEND: " << strPropulsion << endl;
	this->m_Comms.Notify("Propulsion", strPropulsion, MOOSTime());

	return true;
}

bool CPropulsion::OnState(CMOOSMsg& msg){
	if(this->_ctrlPrev.size() == 0) return false;
	msg.Trace(); //print msg in details
	string strState = msg.GetString();

	MOOSValFromString (this->_statePrev.u , strState, "u", false);
	MOOSValFromString (this->_statePrev.v , strState, "v", false);
	MOOSValFromString (this->_statePrev.w , strState, "w", false);
	MOOSValFromString (this->_statePrev.p , strState, "p", false);
	MOOSValFromString (this->_statePrev.q , strState, "q", false);
	MOOSValFromString (this->_statePrev.r , strState, "r", false);
	MOOSValFromString (this->_statePrev.x , strState, "x", false);
	MOOSValFromString (this->_statePrev.y , strState, "y", false);
	MOOSValFromString (this->_statePrev.z , strState, "z", false);
	MOOSValFromString (this->_statePrev.phi , strState, "phi", false);
	MOOSValFromString (this->_statePrev.theta , strState, "theta", false);
	MOOSValFromString (this->_statePrev.psi , strState, "psi", false);

	vector<double> vdblPropulsions;
	this->_fptrPropulsion(this->_ctrlPrev, this->_statePrev, vdblPropulsions);

	string	strPropulsion;
	strPropulsion = MOOSFormat("tau1=%f,tau2=%f,tau3=%f,tau4=%f,tau5=%f,tau6=%f",
		vdblPropulsions[0],
		vdblPropulsions[1],
		vdblPropulsions[2],
		vdblPropulsions[3],
		vdblPropulsions[4],
		vdblPropulsions[5]);
	this->m_Comms.Notify("Propulsion", strPropulsion, MOOSTime());

	return true;

}

